#include "mainwindow.h"
#include "ui_mainwindow.h"

#include <iostream> //标准输入输出流
#include <vtkObject.h>
#include <vtkDataSet.h>
#include <vtkAxesActor.h>



MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    m_cloud_thread = new CloudPointProcess(this);
    init_point_cloud_show();

    cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();// 创建点云（指针）
    QSurfaceFormat::setDefaultFormat(QVTKOpenGLNativeWidget::defaultFormat());
    vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
    vtkSmartPointer<vtkGenericOpenGLRenderWindow> renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
//    设置绕X轴旋转视角 负数为Z->Y旋转，正数为Y->Z旋转
    renderer->GetActiveCamera()->Elevation(-45.0);
    renderWindow->AddRenderer(renderer);


    viewer.reset(new pcl::visualization::PCLVisualizer(renderer, renderWindow, "viewer", false));
    viewer->setupInteractor(ui->openGLWidget_1->interactor(),ui->openGLWidget_1->renderWindow());
    ui->openGLWidget_1->setRenderWindow(viewer->getRenderWindow());
//    ui->openGLWidget_1->update();

    connect(ui->horizontalSlider_point_size, SIGNAL(valueChanged(int)), this, SLOT(pSliderValueChangeed(int)));//改变大小
    connect(ui->horizontalSlider_red, SIGNAL(valueChanged(int)), this, SLOT(redSliderValueChangeed(int)));//颜色改变
    connect(ui->horizontalSlider_green, SIGNAL(valueChanged(int)), this, SLOT(greenSliderValueChangeed(int)));
    connect(ui->horizontalSlider_blue, SIGNAL(valueChanged(int)), this, SLOT(blueSliderValueChangeed(int)));
    connect(ui->horizontalSlider_red, SIGNAL(sliderReleased( )), this, SLOT(rgbSliderReleased( )));//slider滑动
    connect(ui->horizontalSlider_green, SIGNAL(sliderReleased( )), this, SLOT(rgbSliderReleased( )));
    connect(ui->horizontalSlider_blue, SIGNAL(sliderReleased( )), this, SLOT(rgbSliderReleased( )));
    connect(ui->horizontalSlider_point_size, SIGNAL(sliderReleased( )), this, SLOT(pointSliderReleased( )));
    qRegisterMetaType<const pcl::PointCloud<pcl::PointXYZ>::ConstPtr>("const pcl::PointCloud<pcl::PointXYZ>::ConstPtr");
    connect(m_cloud_thread, SIGNAL(send_cloud_point(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr)), this, SLOT(receive_cloud_point(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr)), Qt::QueuedConnection);

}

MainWindow::~MainWindow()
{
    delete ui;
    m_cloud_thread->stop();
    QThread::sleep(1);
}


void MainWindow::init_point_cloud_show()
{
    cloud_total = 100;
    half_cloud_total = int(cloud_total/2);
    clockCount = 0;
    // 显示坐标系的vtk组件
    //    创建一个 vtkAxesActor 对象实例，这是用于显示3D坐标的类。
    vtkSmartPointer<vtkAxesActor> axes_actor = vtkSmartPointer<vtkAxesActor>::New();
    //    设置坐标轴的起始位置为原点（0，0，0）。
    axes_actor->SetPosition(0, 0, 0);
    //    设置坐标轴的长度为2个单位。
    axes_actor->SetTotalLength(2, 2, 2);
    //    设置轴杆类型为实心圆柱体（0表示实心圆柱体，1表示空心圆柱体）。
    axes_actor->SetShaftType(0);
    //    设置轴杆的半径为0.03个单位。
    axes_actor->SetCylinderRadius(0.03);
    //    设置轴标签为可见（0表示不可见，1表示可见）。
    axes_actor->SetAxisLabels(1);
    //    设置轴尖端类型为实心立方体（0表示实心立方体，1表示空心立方体）
    axes_actor->SetTipType(0);
    //    设置X轴的标签
    axes_actor->SetXAxisLabelText("X");
    //    设置x轴轴杆的颜色为白色（R=1，G=1，B=1）。
    axes_actor->GetXAxisShaftProperty()->SetColor(1,0,0);
    //    设置y轴尖端的颜色为白色（R=1，G=1，B=1）。
    axes_actor->GetYAxisTipProperty()->SetColor(1,1,1);

    // 控制坐标系，使之随视角共同变化
    //     创建一个 vtkOrientationMarkerWidget 对象实例，这是用于在QVTKWidget的3D视口中添加标记的类。
    MarkerWidget = vtkSmartPointer<vtkOrientationMarkerWidget>::New();
    //    将之前创建的 vtkAxesActor 对象设置为标记。
    MarkerWidget->SetOrientationMarker(axes_actor);
    //    设置交互器为QVTKWidget的交互器对象。
    MarkerWidget->SetInteractor(ui->openGLWidget_1->interactor());
    //    设置标记的视口位置和大小为渲染区域的前20%（从左下角开始计算）。
    MarkerWidget->SetViewport(0.0, 0.0, 0.2, 0.2);
    //    设置标记为可见（0表示不可见，1表示可见）。
    MarkerWidget->SetEnabled(1);
    //    坐标轴边框颜色设置为红色（R=1，G=0，B=0）
    MarkerWidget->SetOutlineColor(1,0,0);
    //    关闭坐标轴的交互功能
    MarkerWidget->InteractiveOff();

}

void MainWindow::rgbSliderReleased()
{
    //更新点云颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, red, green, blue);
    viewer->updatePointCloud<pcl::PointXYZ>(cloud, single_color, "cloud");
    ui->openGLWidget_1->update();
    ui->openGLWidget_1->renderWindow()->Render();
}

void MainWindow::pointSliderReleased()
{
    //更新点云每个点的尺寸大小
//    获取渲染窗口
    vtkRenderWindow* renderWindow = ui->openGLWidget_1->renderWindow();
//    获取渲染窗口中第一个渲染器
    vtkRenderer* renderer = renderWindow->GetRenderers()->GetFirstRenderer();
//    获取渲染中最后一个actor
    vtkActor* actor = renderer->GetActors()->GetLastActor();
//    设置actor的属性值
    actor->GetProperty()->SetPointSize(this->size);
//    渲染窗口刷新渲染
    renderWindow->Render();
}

void MainWindow::pSliderValueChangeed(int value)
{
    this->size = value;
    ui->lcdNumber_point_size->display(value);
}

void MainWindow::redSliderValueChangeed(int value)
{
    red = value;
    ui->lcdNumber_red->display(value);
}

void MainWindow::greenSliderValueChangeed(int value)
{
    green = value;
    ui->lcdNumber_green->display(value);
}

void MainWindow::blueSliderValueChangeed(int value)
{
    blue = value;
    ui->lcdNumber_blue->display(value);
}

void MainWindow::updateCameraView(double focalPoint[], double position[], double viewUp[])
{
//    获取渲染窗口--渲染者--第一个渲染者--获取正在启用的相机
    vtkCamera* camera = ui->openGLWidget_1->renderWindow()->GetRenderers()->GetFirstRenderer()->GetActiveCamera();
//    设置瞄准点-相机焦点
    camera->SetFocalPoint(focalPoint);
//    设置相机位置
    camera->SetPosition(position);
//    设置视图向上方向
    camera->SetViewUp(viewUp);
    // 刷新渲染窗口
    ui->openGLWidget_1->renderWindow()->Render();
}

void MainWindow::show_point_count(int value)
{
    ui->textEdit_1->setText("点云总数量："+ QString::number(value));
}

void MainWindow::compute_bounding_box()
{
    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
    feature_extractor.setInputCloud(cloud);
    feature_extractor.compute();

    std::vector <float> moment_of_inertia;
    std::vector <float> eccentricity;
    pcl::PointXYZ min_point_AABB;
    pcl::PointXYZ max_point_AABB;

    float major_value, middle_value, minor_value;
    Eigen::Vector3f major_vector, middle_vector, minor_vector;
    Eigen::Vector3f mass_center;

    feature_extractor.getMomentOfInertia(moment_of_inertia);
    feature_extractor.getEccentricity(eccentricity);
    feature_extractor.getAABB(min_point_AABB, max_point_AABB);

    feature_extractor.getEigenValues(major_value, middle_value, minor_value);
    feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);
    feature_extractor.getMassCenter(mass_center);
//    删除之前的AABB形状
    viewer->removeShape("AABB");
    //AABB外接立方体
//    添加一个立方体形状
    viewer->addCube(min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 0.0, 0.0, "AABB", 0);
//    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_POINTS, "AABB");
/*
设定一个形状（shape）的绘制属性，representation是指定形状的绘制方式，material`指定了材质属性，比如颜色，'id'是被指定形状的唯一标识符。
PCL_VISUALIZER_REPRESENTATION_POINT_CLOUD：默认的点云表示方式，点是基于深度缓存技术绘制的，这意味着较远的点会被较近的点遮挡。
PCL_VISUALIZER_REPRESENTATION_SURFACE：表面表示方式，面片会被渲染为三角形网格。
PCL_VISUALIZER_REPRESENTATION_WIREFRAME：线框模式，每个面片都被表示为边框。
PCL_VISUALIZER_REPRESENTATIONContURS：轮廓表示方式，每个面片都会被绘制为一个轮廓线。
PCL_VISUALIZER_REPRESENTATION_VOXELS：三维像素表示方式，点云被转换为体素网格。
*/
//    设置立方体只显示线框
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB");
//    设置立方体显示的颜色
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "AABB");
    // 刷新渲染窗口
    ui->openGLWidget_1->renderWindow()->Render();

}

void MainWindow::compute_min_bounding_box()
{
    pcl::PointXYZ min_point_OBB;
    pcl::PointXYZ max_point_OBB;
    pcl::PointXYZ position_OBB;
    Eigen::Matrix3f rotational_matrix_OBB;
    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
    feature_extractor.setInputCloud(cloud);
    feature_extractor.compute();

    feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
    //OBB外接立方体，最小外接立方体
    Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
    Eigen::Quaternionf quat(rotational_matrix_OBB);
//    删除之前的OBB形状
    viewer->removeShape("OBB");
//    添加一个立方体的形状
    viewer->addCube(position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
//    立方体只显示线框
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");
//    设置立方体显示的颜色
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 1.0, "OBB");
    // 刷新渲染窗口
    ui->openGLWidget_1->renderWindow()->Render();


}

void MainWindow::receive_cloud_point(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud_t)
{
    // 创建一个新的点云指针来存储接收到的数据副本
    pcl::PointCloud<pcl::PointXYZ>::Ptr receivedCloud(new pcl::PointCloud<pcl::PointXYZ>(*cloud_t));
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);

    clockCount++;
    Cloud_p.append(receivedCloud);
    Eigen::Matrix4f translation = Eigen::Matrix4f::Identity();//定义平移矩阵，并初始化为单位阵
    translation(0, 3) = 0;
    translation(1, 3) = clockCount;
    translation(2, 3) = 0;
    pcl::transformPointCloud(*receivedCloud, *cloud_transformed, translation);
    std::string a1 = "Cloud_p[";
    std::string a2 = std::to_string(clockCount);
    std::string a3 = "]";
    std::string aa = a1+a2+a3;
    viewer->addPointCloud(cloud_transformed, aa);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, aa);
    if(clockCount%50 == 0)
    {
        pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud_transformed, "z"); // 按照y字段进行渲染
        viewer->updatePointCloud(cloud_transformed, fildColor, aa);
    }
    else
    {
        viewer->updatePointCloud(cloud_transformed, aa);
    }
    if(Cloud_p.size() > cloud_total)
    {
        Cloud_p.removeFirst();
        std::string b1 = "Cloud_p[";
        std::string b2 = std::to_string(clockCount-Cloud_p.size());
        std::string b3 = "]";
        std::string bb = b1+b2+b3;
        viewer->removePointCloud(bb);
    }
    viewer->resetCamera();
    ui->openGLWidget_1->renderWindow()->Render();
}




void MainWindow::on_pushButton_clicked()
{

    if (pcl::io::loadPCDFile<pcl::PointXYZ>("G:/C++/QT_Project/untitled3/rabbit.pcd", *cloud) == -1) //* 读入PCD格式的文件，如果文件不存在，返回-1
    {
        PCL_ERROR("Couldn't read file test_pcd.pcd \n"); //文件不存在时，返回错误，终止程序。
    }
    int point_cloud_count = cloud->points.size();
    show_point_count(point_cloud_count);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, red, green, blue);
    viewer->removeAllPointClouds();
    viewer->addPointCloud(cloud,"cloud");
    viewer->resetCamera();
    ui->openGLWidget_1->renderWindow()->Render();

    // 显示坐标系的vtk组件
//    创建一个 vtkAxesActor 对象实例，这是用于显示3D坐标的类。
    vtkSmartPointer<vtkAxesActor> axes_actor = vtkSmartPointer<vtkAxesActor>::New();
//    设置坐标轴的起始位置为原点（0，0，0）。
    axes_actor->SetPosition(0, 0, 0);
//    设置坐标轴的长度为2个单位。
    axes_actor->SetTotalLength(2, 2, 2);
//    设置轴杆类型为实心圆柱体（0表示实心圆柱体，1表示空心圆柱体）。
    axes_actor->SetShaftType(0);
//    设置轴杆的半径为0.03个单位。
    axes_actor->SetCylinderRadius(0.03);
//    设置轴标签为可见（0表示不可见，1表示可见）。
    axes_actor->SetAxisLabels(1);
//    设置轴尖端类型为实心立方体（0表示实心立方体，1表示空心立方体）
    axes_actor->SetTipType(0);
//    设置X轴的标签
    axes_actor->SetXAxisLabelText("X");
//    设置x轴轴杆的颜色为白色（R=1，G=1，B=1）。
    axes_actor->GetXAxisShaftProperty()->SetColor(1,0,0);
//    设置y轴尖端的颜色为白色（R=1，G=1，B=1）。
    axes_actor->GetYAxisTipProperty()->SetColor(1,1,1);

    // 控制坐标系，使之随视角共同变化
//     创建一个 vtkOrientationMarkerWidget 对象实例，这是用于在QVTKWidget的3D视口中添加标记的类。
    MarkerWidget = vtkSmartPointer<vtkOrientationMarkerWidget>::New();
//    将之前创建的 vtkAxesActor 对象设置为标记。
    MarkerWidget->SetOrientationMarker(axes_actor);
//    设置交互器为QVTKWidget的交互器对象。
    MarkerWidget->SetInteractor(ui->openGLWidget_1->interactor());
//    设置标记的视口位置和大小为渲染区域的前20%（从左下角开始计算）。
    MarkerWidget->SetViewport(0.0, 0.0, 0.2, 0.2);
//    设置标记为可见（0表示不可见，1表示可见）。
    MarkerWidget->SetEnabled(1);
//    坐标轴边框颜色设置为红色（R=1，G=0，B=0）
    MarkerWidget->SetOutlineColor(1,0,0);
//    关闭坐标轴的交互功能
    MarkerWidget->InteractiveOff();


}


void MainWindow::on_pushButton_rainbow_clicked()
{
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud, "y"); // 按照y字段进行渲染
//    viewer->removeAllPointClouds();
    viewer->updatePointCloud(cloud, fildColor,"cloud");
    viewer->resetCamera();
    ui->openGLWidget_1->update();
    ui->openGLWidget_1->renderWindow()->Render();
}


void MainWindow::on_pushButton_front_clicked()
{
    //xoz y正 平面展示
    vtkPolyData* polydata = vtkPolyData::SafeDownCast(ui->openGLWidget_1->renderWindow()->GetRenderers()->GetFirstRenderer()->GetActors()->GetLastActor()->GetMapper()->GetInput());
    double center[3];
    polydata->GetCenter(center);
    double bounds[6];
    polydata->GetBounds(bounds);
    double max_range = std::max({bounds[1]-bounds[0], bounds[3]-bounds[2], bounds[4]});
    double cam_distance = max_range / tan(45.0*vtkMath::Pi()/360.0);
    double position[3] = {center[0], center[1]-cam_distance, center[2]};
    double viewUp[3] = {0.0, 0.0, 1.0};
    updateCameraView(center, position, viewUp);

}


void MainWindow::on_pushButton_back_clicked()
{
    //xoz y负 平面展示
    vtkPolyData* polydata = vtkPolyData::SafeDownCast(ui->openGLWidget_1->renderWindow()->GetRenderers()->GetFirstRenderer()->GetActors()->GetLastActor()->GetMapper()->GetInput());
    double center[3];
    polydata->GetCenter(center);
    double bounds[6];
    polydata->GetBounds(bounds);
    double max_range = std::max({bounds[0]-bounds[1], bounds[3]-bounds[2], bounds[5]-bounds[4]});
    double cam_distance = max_range / tan(45.0*vtkMath::Pi()/360.0);
    double position[3] = {center[0], center[1]+cam_distance, center[2]};
    double viewUp[3] = {0.0, 0.0, 1.0};
    updateCameraView(center, position, viewUp);

}


void MainWindow::on_pushButton_left_clicked()
{
    //yoz x正 平面展示
    vtkPolyData* polydata = vtkPolyData::SafeDownCast(ui->openGLWidget_1->renderWindow()->GetRenderers()->GetFirstRenderer()->GetActors()->GetLastActor()->GetMapper()->GetInput());
    double center[3];
    polydata->GetCenter(center);
    double bounds[6];
    polydata->GetBounds(bounds);
    double max_range = std::max({bounds[0], bounds[3]-bounds[2], bounds[5]-bounds[4]});
    double cam_distance = max_range / tan(45.0*vtkMath::Pi()/360.0);
    double position[3] = {center[0]-cam_distance, center[1], center[2]};
    double viewUp[3] = {0.0, 0.0, 1.0};
    updateCameraView(center, position, viewUp);

}


void MainWindow::on_pushButton_right_clicked()
{
    //yoz x负 平面展示
    vtkPolyData* polydata = vtkPolyData::SafeDownCast(ui->openGLWidget_1->renderWindow()->GetRenderers()->GetFirstRenderer()->GetActors()->GetLastActor()->GetMapper()->GetInput());
    double center[3];
    polydata->GetCenter(center);
    double bounds[6];
    polydata->GetBounds(bounds);
    double max_range = std::max({bounds[1], bounds[3]-bounds[2], bounds[5]-bounds[4]});
    double cam_distance = max_range / tan(45.0*vtkMath::Pi()/360.0);
    double position[3] = {center[0]+cam_distance, center[1], center[2]};
    double viewUp[3] = {0.0, 0.0, 1.0};
    updateCameraView(center, position, viewUp);

}


void MainWindow::on_pushButton_top_clicked()
{
    //xoy z负 平面展示
    //获取渲染窗口的第一个渲染者的最后一个actor的点云数据
    vtkPolyData* polydata = vtkPolyData::SafeDownCast(ui->openGLWidget_1->renderWindow()->GetRenderers()->GetFirstRenderer()->GetActors()->GetLastActor()->GetMapper()->GetInput());
    double center[3];
    //获取点云数据的中心点坐标 作为相机的瞄准点
    polydata->GetCenter(center);
    double bounds[6];
    //获取点云的上下左右前后的边界值
    polydata->GetBounds(bounds);
    //计算边界值范围
    double max_range = std::max({bounds[1]-bounds[0], bounds[3]-bounds[2], bounds[5]-bounds[4]});
    double cam_distance = max_range / tan(45.0*vtkMath::Pi()/360.0);
    double cam_height = cam_distance * tan(vtkMath::Pi()/4.0);
    double position[3] = {center[0], center[1], center[2]+cam_height};
    double viewUp[3] = {0.0, 1.0, 0.0};
    updateCameraView(center, position, viewUp);

}


void MainWindow::on_pushButton_down_clicked()
{
    //xoy z正 平面展示
    vtkPolyData* polydata = vtkPolyData::SafeDownCast(ui->openGLWidget_1->renderWindow()->GetRenderers()->GetFirstRenderer()->GetActors()->GetLastActor()->GetMapper()->GetInput());
    double center[3];
    polydata->GetCenter(center);
    double bounds[6];
    polydata->GetBounds(bounds);
    double max_range = std::max({bounds[1]-bounds[0], bounds[5]-bounds[4], bounds[2]-bounds[3]});
    double cam_distance = max_range / tan(45.0*vtkMath::Pi()/360.0);
    double cam_height = cam_distance * tan(vtkMath::Pi()/4.0);
    double position[3] = {center[0], center[1], center[2]-cam_height};
    double viewUp[3] = {0.0, -1.0, 0.0};
    updateCameraView(center, position, viewUp);

}


void MainWindow::on_checkBox_CoordinateSystem_stateChanged(int arg1)
{
    if(arg1 == 0){
        MarkerWidget->SetEnabled(0);
    }else {
        MarkerWidget->SetEnabled(1);
    }
    // 刷新渲染窗口
    ui->openGLWidget_1->renderWindow()->Render();
}


void MainWindow::on_checkBox_stateChanged(int arg1)
{
    if(arg1 == 0){
        viewer->setBackgroundColor(0,0,0);
    }else {
        viewer->setBackgroundColor(0.9,0.9,0.9);
    }
    // 刷新渲染窗口
    ui->openGLWidget_1->renderWindow()->Render();
}


void MainWindow::on_checkBox_bbox_stateChanged(int arg1)
{
    if(arg1){
        compute_bounding_box();
    }else{
        viewer->removeShape("AABB");
        ui->openGLWidget_1->renderWindow()->Render();
    }

}


void MainWindow::on_checkBox_min_bbox_stateChanged(int arg1)
{
    if(arg1){
        compute_min_bounding_box();
    }else{
        viewer->removeShape("OBB");
        ui->openGLWidget_1->renderWindow()->Render();
    }

}


void MainWindow::on_pushButton_update_cloud_stream_clicked()
{
    m_cloud_thread->start();
}


void MainWindow::on_pushButton_close_cloud_point_stream_clicked()
{
    m_cloud_thread->stop();
}

